#include <stdio.h>
#include "Wheel.h"
#include "espFFB.h"
#include "encoder.h"
#include "Motor.h"
#include "MathHelper.h"
#include "esp_timer.h"
#include "port.h"
#include "ParameterManager.h"
#include "math.h"
// NVS_Config WheelConfig ={
//     .Enable = 0b11100000,
//     .MaxFfbGain = 255,
//     .EncoderCnt = 2000,
//     .WheelAngleLimit = 540 //单位是度
// };
//int32_t wheelAngle = 0;
//这是应用层代码，通过驱动实现力反馈方向盘
// static void GetInputTask(void *pvPara){
//     //TODO: 因为GlobalConfig的结构变了，所以这里也得修复
//     static JoystickReport last = {0};
//     JoystickReport Report = {0};
//     // static JoystickReport report = {0}; 
//     while (1)
//     {
//         //获取传感器输入计数值
//         /*在这里最小值，X轴的范围是0~1024，编码器旋转一圈计数值是编码器光栅的四倍，映射到方向盘的整个旋转角度*/
//         //int32_t wheelAngle = (encoder_get_count()/(float)(GlobalConfig.EncoderCnt*4) + Cycle) * 36000.0f;
//         worldModel.AngleX = (int32_t)(GetMotorPos(0)*36000.0);
//         //worldModel.AxisVelocity[0] = GetMotorSpeed(0);
//         // 这里的转换看似稍微有一点危险，实则问题不大,因为最小值0，最大值是1024
//         int16_t value_axis_x = (int16_t)linear_map(worldModel.AngleX,-.XAngleLimit/ 2 * 100,GlobalConfig.XAngleLimit / 2 * 100,-32768,32767);
//         Report.axes.X = value_axis_x;
//         // 设置按钮状态（例如按钮1和按钮3按下，其他按钮未按下）
//         // worldModel.JoyStick.buttons.button1 = 1;  // 所有按钮都默认为未按下
//         // // 其他按钮都默认为未按下
//         // // 设置方向键状态
//         // worldModel.JoyStick.hat_switch.hat_switch = HAT_SWITCH_NORTH;  // 没有按下
        
//         // // 设置摇杆的 X, Y, Z 和 Rz 值
//         // worldModel.JoyStick.analog_stick.Y = 1024; 
//         // worldModel.JoyStick.analog_stick.Z = 512;
//         // worldModel.JoyStick.analog_stick.Rz = 256;
        
//         if(memcmp(&last,&Report,sizeof(JoystickReport))!=0) {

//             tud_hid_report(1,&Report,sizeof(JoystickReport));
//             memcpy(&last,&Report,sizeof(JoystickReport));
//         }   
//         vTaskDelay(pdMS_TO_TICKS(20));
//     }
// }
static void CanRecvTask(void *para){
    if(CanInit()!=1){
        printf("Can start failed!");
        vTaskDelete(NULL);
    }
    while (1)
    {
        CanRecvMessage();
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    
}
esp_timer_handle_t timer_can;
void WheelInit(void){
    
    // esp_timer_create_args_t timer_args = {
    //     .callback = CanRecvTask,
    //     .arg = NULL,
    //     .name = "effect update"
    // };
    // if(esp_timer_create(&timer_args, &timer_can) == ESP_OK){
    //     esp_timer_start_periodic(timer_can, 20000); 
    // }
    // else{
    //     printf("err\n");
    // }
    //encoder_init();
    //CanInit();
    //xTaskCreatePinnedToCore(GetInputTask,"GetInput task",1024,NULL,2,NULL,0);
    //xTaskCreatePinnedToCore(CanRecvTask,"CanRecv task",2048,NULL,2,NULL,1);
    //vTaskDelay(pdMS_TO_TICKS(10));
    //MotorInit();
}

/**
 * 应用物理效果：摩擦力、阻尼
 * 从全局状态读取当前速度和配置参数
 * @param force 力输出值（输入输出参数）
 */
// 在中心位置附近设置死区，避免微小震荡
void apply_physical_effects(float* force)
{
    if (force == NULL) return;
    
    float current_velocity = GetAxisSpeed(AXIS_X);
    float current_angle = GetAxisPos(AXIS_X);
    
    float friction = g_ParaList.friction;
    float damping = g_ParaList.damp;
    float spring_constant = g_ParaList.spring;
    float deadzone_angle = 2.0f; // 死区角度，中心±2度内无弹簧力
    
    float total_additional_force = 0.0f;
    
    // 摩擦力
    float friction_force = 0.0f;
    if (fabsf(current_velocity) > 0.1f) {
        friction_force = -friction * (current_velocity > 0 ? 1.0f : -1.0f);
    } else if (fabsf(*force) < friction) {
        friction_force = -*force;
    }
    total_additional_force += friction_force;
    
    // 阻尼力
    total_additional_force += -damping * current_velocity;
    
    // 带死区的弹簧力
    if (fabsf(current_angle) > deadzone_angle) {
        float effective_angle = current_angle - (current_angle > 0 ? deadzone_angle : -deadzone_angle);
        float spring_force = -spring_constant * effective_angle;
        total_additional_force += spring_force;
    }
    
    *force += total_additional_force;
}

/**
 * 应用力限制和角度限制
 * 从全局状态读取当前角度和配置参数
 * @param force 输入力值
 * @return 调整后的力值
 */
float apply_force_angle_limits(float force)
{
    float result_force = force;
    
    // 获取当前角度
    float current_angle = GetAxisPos(AXIS_X); // 你的全局角度获取函数
    
    // 获取配置参数
    float max_force = 100000;         // 你的全局最大力参数
    float max_angle = g_ParaList.XAngleLimit;         // 你的全局最大角度参数
    float spring_constant = 100;
    
    // 1. 应用力限制
    if (result_force > max_force) {
        result_force = max_force;
    } else if (result_force < -max_force) {
        result_force = -max_force;
    }
    
    // 2. 应用角度限制
    if (current_angle > max_angle) {
        if (result_force > 0) result_force = 0;
        result_force -= (current_angle - max_angle) * spring_constant;
    } else if (current_angle < -max_angle) {
        if (result_force < 0) result_force = 0;
        result_force -= (current_angle + max_angle) * spring_constant;
    }
    
    return result_force;
}